#python3 generate_data_stamp.py --data_dir /media/gwm/152E776914CDED89/linux_work/open_dataset/nclt/20120108/ --output data_stamp.csv
#python3 generate_data_stamp.py --data_dir /media/zl/1E2E59842E5955BD/SLAM/data/2012-01-08 --output data_stamp.csv


import os
import struct
import csv
import argparse

def parse_hokuyo_bin(bin_path):
    """解析Hokuyo 30m激光雷达二进制文件"""
    timestamps = []
    try:
        with open(bin_path, 'rb') as f:
            # 跳过可能的文件头（根据实际格式调整）
            # header = f.read(8)  # 示例：跳过4字节头
            while True:
                data = f.read(8)
                if not data or len(data) < 8:
                    break
                timestamp = struct.unpack('<Q', data)[0]
                timestamps.append(str(timestamp))
    except Exception as e:
        print(f"Hokuyo解析错误: {str(e)}")
    return timestamps

def process_sensor(data_dir, filename, sensor_type):
    """处理单个传感器数据"""
    filepath = os.path.join(data_dir, filename)
    if not os.path.exists(filepath):
        print(f"警告：文件不存在 {filename}")
        return []

    print(f"处理 {sensor_type} ({filename})...")
    
    if filename == 'hokuyo_30m.bin':
        return [[ts, sensor_type] for ts in parse_hokuyo_bin(filepath)]
    
    elif filename == 'wheels.csv':
        with open(filepath) as f:
            reader = csv.reader(f)
            next(reader)
            return [[row[0], 'Wheel_Velocity'] for row in reader if row]
    
    elif filename.endswith('.csv'):
        with open(filepath) as f:
            reader = csv.reader(f)
            next(reader)
            return [[row[0], sensor_type] for row in reader if row]
    
    elif filename.endswith('.txt'):
        with open(filepath) as f:
            return [[line.strip(), sensor_type] for line in f if line.strip().isdigit()]

def main():
    parser = argparse.ArgumentParser(description='NCLT时间戳提取器')
    parser.add_argument('--data_dir', required=True, help='数据目录路径')
    parser.add_argument('--output', default='data_stamp.csv', help='输出文件名')
    args = parser.parse_args()

    output_path = os.path.join(args.data_dir, args.output)

    sensors = {
        'ms25.csv': 'Microstrain_IMU',
        'hokuyo_30m.bin': 'Hokuyo_30m',
        'wheels.csv': 'Wheel_Velocity',
        'odometry_mu_100hz.csv': 'Vehicle_Odometry'
    }

    all_data = []
    for filename, sensor_type in sensors.items():
        all_data.extend(process_sensor(args.data_dir, filename, sensor_type))

    all_data.sort(key=lambda x: int(x[0]))

    os.makedirs(os.path.dirname(output_path) or '.', exist_ok=True)

    with open(output_path, 'w', newline='') as f:
        csv.writer(f).writerows(all_data)
    print(f"生成 {args.output} 完成，共 {len(all_data)} 条记录")

if __name__ == '__main__':
    main()

